#include "vehicle_service.h"

#include <algorithm>
#include <chrono>
#include <utility>
#include <QMetaType>

#include <QString>
#include <QStringList>

#include "application/map/auto_door/auto_door_manager.h"
#include "application/map/area/area_manager.h"
#include "application/map/map_application_service.h"
#include "infrastructure/vehicle/mklm/mklm_command_impl.h"
#include "infrastructure/vehicle/mklm/mklm_status_provider.h"

namespace application {
namespace vehicle {

VehicleService::VehicleService(
    std::shared_ptr<infrastructure::services::CommonService> commonService,
    std::shared_ptr<VehicleOperationDispatcher> dispatcher,
    std::shared_ptr<MapApplicationService> mapService,
    QObject* parent)
    : QObject(parent),
      m_logger(commonService ? commonService->getLogger("VehicleService")
                             : nullptr),
      m_dispatcher(std::move(dispatcher)),
      m_mapService(std::move(mapService)),
      m_healthCheckTimer(new QTimer(this)) {
  if (m_logger) {
    m_logger->info("VehicleService initialized");
  }

  qRegisterMetaType<VehicleService::AutoChargeReason>("VehicleService::AutoChargeReason");

  if (m_healthCheckTimer) {
    m_healthCheckTimer->setInterval(1000);
    m_healthCheckTimer->setTimerType(Qt::VeryCoarseTimer);
    connect(m_healthCheckTimer, &QTimer::timeout, this,
            &VehicleService::onHealthCheckTimeout);
    m_healthCheckTimer->start();
  }
}

// ==================== 车辆管理 ====================

kernel::Result<void> VehicleService::registerVehicle(
    int agvNumber, std::shared_ptr<kernel::agv::IAgvCommand> commandImpl,
    kernel::agv::AgvType type) {
  if (vehicleExists(agvNumber)) {
    if (m_logger) {
      m_logger->warning(QString("车辆已存在: %1").arg(agvNumber));
    }
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidOperation,
                                         "Vehicle already registered");
  }

  // 创建新的 AgvEntity
  // 使用 agvNumber 作为 id 和 number
  auto agvEntity =
      std::make_unique<kernel::agv::AgvEntity>(agvNumber, agvNumber);
  agvEntity->setType(type);

  // 注册到 dispatcher
  m_dispatcher->registerCommandImplementation(agvNumber, commandImpl);

  // 添加到车辆映射
  m_vehicles[agvNumber] = std::move(agvEntity);

  if (m_logger) {
    m_logger->info(QString("注册车辆: %1").arg(agvNumber));
  }

  emit vehicleRegistered(agvNumber);
  return kernel::Result<void>::success();
}

kernel::Result<void> VehicleService::registerVehicleStatusProvider(
    int agvNumber,
    std::shared_ptr<kernel::agv::IAgvStatusProvider> statusProvider) {
  if (!vehicleExists(agvNumber)) {
    if (m_logger) {
      m_logger->warning(
          QString("车辆不存在，无法注册状态提供者: %1").arg(agvNumber));
    }
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidOperation,
                                         "Vehicle not registered");
  }

  // 设置状态更新回调
  statusProvider->setStatusUpdateCallback(
      [this, agvNumber](const kernel::dtos::AgvStatusDTO& status) {
        this->handleStatusUpdate(agvNumber, status);
      });

  // 启动状态提供者
  auto startResult = statusProvider->start();
  if (!startResult.isSuccess()) {
    if (m_logger) {
      m_logger->error(QString("启动状态提供者失败: AGV %1, 错误: %2")
                          .arg(agvNumber)
                          .arg(startResult.getErrorMessage()));
    }
    return startResult;
  }

  // 保存到映射
  m_statusProviders[agvNumber] = statusProvider;

  if (m_logger) {
    m_logger->info(QString("注册状态提供者: AGV %1").arg(agvNumber));
  }

  return kernel::Result<void>::success();
}

void VehicleService::unregisterVehicle(int agvNumber) {
  if (!vehicleExists(agvNumber)) {
    return;
  }

  clearAgvAreaOccupancy(agvNumber);

  // 停止并移除状态提供者
  auto statusProviderIt = m_statusProviders.find(agvNumber);
  if (statusProviderIt != m_statusProviders.end()) {
    statusProviderIt->second->stop();
    m_statusProviders.erase(statusProviderIt);
  }

  m_dispatcher->unregisterCommandImplementation(agvNumber);
  m_vehicles.erase(agvNumber);

  if (m_logger) {
    m_logger->info(QString("注销车辆: %1").arg(agvNumber));
  }

  emit vehicleUnregistered(agvNumber);
}

kernel::agv::AgvEntity* VehicleService::getVehicle(int agvNumber) {
  auto it = m_vehicles.find(agvNumber);
  if (it != m_vehicles.end()) {
    return it->second.get();
  }
  return nullptr;
}

const kernel::agv::AgvEntity* VehicleService::getVehicle(int agvNumber) const {
  auto it = m_vehicles.find(agvNumber);
  if (it != m_vehicles.end()) {
    return it->second.get();
  }
  return nullptr;
}

std::vector<kernel::agv::AgvEntity*> VehicleService::getAllVehicles() {
  std::vector<kernel::agv::AgvEntity*> vehicles;
  vehicles.reserve(m_vehicles.size());
  for (auto& pair : m_vehicles) {
    vehicles.push_back(pair.second.get());
  }
  return vehicles;
}

size_t VehicleService::getVehicleCount() const { return m_vehicles.size(); }

// ==================== 车辆导航操作 ====================

kernel::Result<void> VehicleService::moveVehicleToStations(
    int agvNumber, const std::vector<std::string>& stations,
    const std::string& operation) {
  // 参考 agv.cpp:81-273 PublishMoveTaskList()

  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  if (stations.empty()) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Station list is empty");
  }

  std::vector<std::string> targetStations = stations;

  // 1. 检查AGV是否在导航中（参考 agv.cpp:128-227）
  if (isVehicleNavigating(agvNumber)) {
    // 1.1 路径比对（参考 agv.cpp:146-171）
    if (isSameRoute(agv->getUnfinishedPath(), targetStations)) {
      if (m_logger) {
        m_logger->info(QString("AGV %1 路径相同，不下发").arg(agvNumber));
      }
      return kernel::Result<void>::success();
    }

    // 1.2 处理finished_path，计算起始索引（参考 agv.cpp:173-188）
    int startIndex =
        calculateRouteStartIndex(agv->getFinishedPath(), targetStations);

    if (startIndex > 0 &&
        startIndex < static_cast<int>(targetStations.size())) {
      // 截取站点列表，从起始索引开始
      std::vector<std::string> truncated(
          targetStations.begin() + startIndex, targetStations.end());

      // 截断后站点数不足2个，回退到下发完整路径
      if (truncated.size() < 2) {
        if (m_logger) {
          m_logger->info(QString("AGV %1 截断后站点数不足2（剩余%2个），回退下发完整路径")
                             .arg(agvNumber)
                             .arg(truncated.size()));
        }
        // 不截断，保持 targetStations 为原始路径
      } else {
        targetStations = std::move(truncated);
        if (m_logger) {
          m_logger->debug(QString("AGV %1 从索引 %2 开始发送路径")
                              .arg(agvNumber)
                              .arg(startIndex));
        }
      }
    }
  }

  // 2. 通过 dispatcher 发送命令
  auto result =
      m_dispatcher->moveToStationList(*agv, targetStations, operation);

  // 3. 更新AGV状态（参考 agv.cpp:268-272）
  if (result.isSuccess()) {
    agv->updateLastBusyTime();
    agv->setBusyStatus(true);

    if (m_logger) {
      QStringList pathList;
      for (const auto& station : targetStations) {
        pathList.append(QString::fromStdString(station));
      }
      QString pathInfo = pathList.isEmpty()
          ? QString("-")
          : pathList.join(" -> ");
      m_logger->info(QString("AGV %1 移动任务已下发，站点数: %2，路径: [%3]")
                         .arg(agvNumber)
                         .arg(targetStations.size())
                         .arg(pathInfo));
    }
  } else {
    if (m_logger) {
      m_logger->error(QString("AGV %1 移动任务下发失败: %2")
                          .arg(agvNumber)
                          .arg(result.getErrorMessage()));
    }
  }

  return result;
}

kernel::Result<void> VehicleService::moveVehicleToStation(
    int agvNumber, const std::string& sourceId, const std::string& targetId) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->moveToStation(*agv, sourceId, targetId);
}

kernel::Result<void> VehicleService::clearVehicleTask(int agvNumber) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  kernel::Result<void> result = kernel::Result<void>::success();
  if (agv->getCurrentStation().empty()) {
    if (m_logger) {
      m_logger->info(QString("Skip cancel command for AGV %1: not on station")
                         .arg(agvNumber));
    }
  } else {
    result = m_dispatcher->cancelTask(*agv);
  }

  // 更新车辆状态
  agv->setControlledByDispatcher(false);
  agv->setBusyStatus(false);

  return result;
}

kernel::Result<void> VehicleService::pauseVehicleTask(int agvNumber) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->pauseTask(*agv);
}

kernel::Result<void> VehicleService::resumeVehicleTask(int agvNumber) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->resumeTask(*agv);
}

// ==================== 车辆硬件控制 ====================

kernel::Result<void> VehicleService::controlVehicleJack(int agvNumber,
                                                        bool raise) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->controlJack(*agv, raise);
}

kernel::Result<void> VehicleService::sendVehicleToCharge(
    int agvNumber, const std::string& chargePoint) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->startCharging(*agv, chargePoint);
}

kernel::Result<void> VehicleService::stopVehicleCharging(int agvNumber) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->stopCharging(*agv);
}

kernel::Result<void> VehicleService::controlDoor(int agvNumber, int doorId,
                                                 bool open) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->controlDoor(*agv, doorId, open);
}

kernel::Result<void> VehicleService::switchMap(
    int agvNumber,
    const std::string& mapName,
    const std::string& switchPoint) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->switchMap(*agv, mapName, switchPoint);
}

// ==================== DTO 查询方法（用于 Presentation 层）====================

std::vector<kernel::dtos::AgvInfoDTO> VehicleService::getAllAgvs() {
  std::vector<kernel::dtos::AgvInfoDTO> result;
  auto vehicles = getAllVehicles();

  for (auto* agv : vehicles) {
    result.push_back(entityToInfoDTO(agv));
  }

  return result;
}

kernel::dtos::AgvInfoDTO VehicleService::getAgv(int agvNumber) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::dtos::AgvInfoDTO();
  }

  return entityToInfoDTO(agv);
}

bool VehicleService::startCharging(int agvNumber,
                                   const std::string& chargePoint) {
  auto result = sendVehicleToCharge(agvNumber, chargePoint);
  return result.isSuccess();
}

bool VehicleService::stopCharging(int agvNumber) {
  auto result = stopVehicleCharging(agvNumber);
  return result.isSuccess();
}

bool VehicleService::cancelAgvTask(int agvNumber) {
  auto result = clearVehicleTask(agvNumber);
  return result.isSuccess();
}

bool VehicleService::controlJack(int agvNumber, bool raise) {
  auto result = controlVehicleJack(agvNumber, raise);
  return result.isSuccess();
}

bool VehicleService::pauseAgvTask(int agvNumber) {
  auto result = pauseVehicleTask(agvNumber);
  return result.isSuccess();
}

bool VehicleService::resumeAgvTask(int agvNumber) {
  auto result = resumeVehicleTask(agvNumber);
  return result.isSuccess();
}

void VehicleService::resetAutoChargeTimer(int agvNumber) {
  auto& state = m_autoChargeStates[agvNumber];
  state.idleChargeTriggered = false;
  state.lowBatteryTriggered = false;
  state.idleResetTime = std::chrono::system_clock::now();

  if (m_logger) {
    m_logger->debug(QString("重置AGV %1的自动充电定时器").arg(agvNumber));
  }
}

// ==================== 车辆查询 ====================

std::vector<kernel::agv::AgvEntity*> VehicleService::getIdleVehicles() {
  std::vector<kernel::agv::AgvEntity*> idleVehicles;

  for (auto& pair : m_vehicles) {
    auto* agv = pair.second.get();

    // 空闲条件：不忙碌、不受调度控制、未充电、在线
    if (!agv->isBusy() && !agv->isControlledByDispatcher() &&
        !agv->isCharging() && agv->isOnline() &&
        !agv->getCurrentStation().empty()) {
      idleVehicles.push_back(agv);
    }
  }

  // 优先级排序：电量高的优先
  std::sort(
      idleVehicles.begin(), idleVehicles.end(),
      [](const kernel::agv::AgvEntity* a, const kernel::agv::AgvEntity* b) {
        return a->getBatteryLevel() > b->getBatteryLevel();
      });

  return idleVehicles;
}

std::vector<kernel::agv::AgvEntity*> VehicleService::getVehiclesOnMap(
    const std::string& mapName) {
  std::vector<kernel::agv::AgvEntity*> vehicles;

  for (auto& pair : m_vehicles) {
    auto* agv = pair.second.get();
    if (agv->getCurrentMap() == mapName) {
      vehicles.push_back(agv);
    }
  }

  return vehicles;
}

std::vector<VehicleNavRequest> VehicleService::getAllNavRequests(
    const std::string& mapName, bool enforce) {
  std::vector<VehicleNavRequest> requests;

  int taskCount = 0;
  for (auto& pair : m_vehicles) {
    auto* agv = pair.second.get();

    if (!enforce && agv->getTaskRequestCount() == 0) {
      continue;
    }

    if (agv->getCurrentMap() != mapName) continue;
    if (agv->isCharging()) continue;
    if (!agv->isOnline()) continue;
    if (agv->isLocked()) continue;

    taskCount++;
  }

  if (taskCount == 0 && !enforce) {
    return requests;
  }

  for (auto& pair : m_vehicles) {
    auto* agv = pair.second.get();

    if (agv->getCurrentMap() != mapName) continue;
    if (agv->isCharging()) continue;
    if (!agv->isOnline()) continue;
    if (agv->isLocked()) continue;

    std::string startStation, goalStation;
    getVehicleNavTask(agv->getNumber(), startStation, goalStation);

    if (startStation.empty() || goalStation.empty()) {
      if (m_logger) {
        m_logger->debug(
            QString("车辆 %1 起点或终点为空").arg(agv->getNumber()));
      }
      continue;
    }

    VehicleNavRequest request;
    request.agvNumber = agv->getNumber();
    request.startStation = startStation;
    request.goalStation = goalStation;
    requests.push_back(request);

    // 重置任务请求计数
    agv->resetTaskRequestCount();
  }

  return requests;
}

std::string VehicleService::getVehicleCurrentStation(int agvNumber) const {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return "";
  }
  return agv->getCurrentStation();
}

std::string VehicleService::getVehicleTargetStation(int agvNumber) const {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return "";
  }
  return agv->getTargetStation();
}

bool VehicleService::isVehicleNavigating(int agvNumber) const {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return false;
  }

  auto navStatus = agv->getNavStatus();
  auto taskType = agv->getTaskType();

  return (navStatus == kernel::agv::NavStatus::Running ||
          navStatus == kernel::agv::NavStatus::Suspended) &&
         taskType == kernel::agv::TaskType::PathNavToStation;
}

void VehicleService::setVehicleNavTask(int agvNumber, const std::string& goal) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return;
  }

  agv->setControlledByDispatcher(true);
  agv->incrementTaskRequestCount();
  agv->setTargetStation(goal);
}

void VehicleService::getVehicleNavTask(int agvNumber, std::string& startStation,
                                       std::string& goalStation) const {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    startStation = "";
    goalStation = "";
    return;
  }

  // 如果不受调度控制
  if (!agv->isControlledByDispatcher()) {
    // 完成态或取消态，使用当前站点
    if (agv->getTaskType() == kernel::agv::TaskType::PathNavToStation &&
        (agv->getNavStatus() == kernel::agv::NavStatus::Completed ||
         agv->getNavStatus() == kernel::agv::NavStatus::Canceled)) {
      if (!agv->getCurrentStation().empty()) {
        // 移除前缀 "LM"
        std::string currentStation = agv->getCurrentStation();
        if (currentStation.size() > 2 && currentStation.substr(0, 2) == "LM") {
          startStation = currentStation.substr(2);
        } else {
          startStation = currentStation;
        }
      }
    }
  } else {
    // 受调度控制中
    if (agv->getNavStatus() == kernel::agv::NavStatus::Running &&
        agv->getTaskType() == kernel::agv::TaskType::PathNavToStation) {
      std::string stationToUse;
      if (!agv->getCurrentStation().empty()) {
        stationToUse = agv->getCurrentStation();
      } else if (!agv->getLastStation().empty()) {
        stationToUse = agv->getLastStation();
      }

      // 移除前缀
      if (!stationToUse.empty() && stationToUse.size() > 2) {
        if (stationToUse.substr(0, 2) == "LM") {
          startStation = stationToUse.substr(2);
        } else if (stationToUse.substr(0, 2) != "AP") {
          startStation = stationToUse;
        }
      }
    }

    // 完成态或取消态
    if ((agv->getTaskType() == kernel::agv::TaskType::PathNavToStation ||
         agv->getTaskType() == kernel::agv::TaskType::Other) &&
        (agv->getNavStatus() == kernel::agv::NavStatus::Completed ||
         agv->getNavStatus() == kernel::agv::NavStatus::Canceled)) {
      if (!agv->getCurrentStation().empty()) {
        std::string currentStation = agv->getCurrentStation();
        if (currentStation.size() > 2 && currentStation.substr(0, 2) == "LM") {
          startStation = currentStation.substr(2);
        } else {
          startStation = currentStation;
        }
      }
    }
  }

  goalStation = agv->getTargetStation();
  if (goalStation.empty()) {
    goalStation = agv->getCurrentStation();
    if (goalStation.size() > 2 && goalStation.substr(0, 2) == "LM") {
      goalStation = goalStation.substr(2);
    }
  }
}

kernel::Result<void> VehicleService::requestVehicleMap(int agvNumber) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    return kernel::Result<void>::failure(kernel::ErrorCode::InvalidParameter,
                                         "Vehicle not found");
  }

  return m_dispatcher->requestCurrentMap(*agv);
}

kernel::Result<void> VehicleService::resendLastCommand(int agvNumber) {
  // 重发命令功能需要在 infrastructure 层实现
  // 这里返回成功，实际实现需要保存最后一条命令
  if (m_logger) {
    m_logger->info(QString("重发命令: %1").arg(agvNumber));
  }
  return kernel::Result<void>::success();
}

// ==================== 车辆状态管理 ====================

void VehicleService::updateVehicleOnlineStatus() {
  auto now = std::chrono::system_clock::now();
  const int offlineThresholdSeconds = 5;

  for (auto& pair : m_vehicles) {
    auto* agv = pair.second.get();
    auto lastUploadTime = agv->getLastUploadTime();

    auto duration =
        std::chrono::duration_cast<std::chrono::seconds>(now - lastUploadTime);

    bool shouldBeOnline = duration.count() <= offlineThresholdSeconds;

    if (agv->isOnline() != shouldBeOnline) {
      agv->setOnlineStatus(shouldBeOnline);
      emit vehicleStatusChanged(agv->getNumber());
    }
  }
}

void VehicleService::manageAutoCharging() {
  auto now = std::chrono::system_clock::now();
  const int idleChargeThresholdSeconds = 600;  // 10分钟
  const int idleChargeBatteryLevel = 80;
  const int lowBatteryLevel = 40;

  for (auto& pair : m_vehicles) {
    auto* agv = pair.second.get();

    if (!agv->isOnline()) continue;
    if (agv->getCurrentStation().empty()) continue;

    auto& chargeState = m_autoChargeStates[agv->getNumber()];
    const int batteryLevel = agv->getBatteryLevel();

    // 空闲充电（10分钟空闲，电量<80%）
    auto idleBaseline = agv->getLastBusyTime();
    if (chargeState.idleResetTime.has_value() &&
        chargeState.idleResetTime.value() > idleBaseline) {
      idleBaseline = chargeState.idleResetTime.value();
    }
    auto idleDuration = std::chrono::duration_cast<std::chrono::seconds>(
        now - idleBaseline);

    const bool shouldIdleCharge = idleDuration.count() > idleChargeThresholdSeconds &&
        !agv->isControlledByDispatcher() && !agv->isCharging() &&
        batteryLevel < idleChargeBatteryLevel;

    if (shouldIdleCharge) {
      if (m_logger) {
        m_logger->info(QString("车辆 %1 触发空闲充电").arg(agv->getNumber()));
      }
      if (!chargeState.idleChargeTriggered) {
        emit autoChargeRequested(agv->getNumber(), AutoChargeReason::IdleCharge, batteryLevel);
        chargeState.idleChargeTriggered = true;
      }
    } else {
      chargeState.idleChargeTriggered = false;
    }

    // 低电量充电（电量<40%）
    const bool lowBattery = batteryLevel < lowBatteryLevel &&
        !agv->isControlledByDispatcher() && !agv->isCharging() && batteryLevel!=0;

    if (lowBattery) {
      if (m_logger) {
        m_logger->warning(
            QString("车辆 %1 电量低，需要充电").arg(agv->getNumber()));
      }
      if (!chargeState.lowBatteryTriggered) {
        emit autoChargeRequested(agv->getNumber(), AutoChargeReason::LowBattery, batteryLevel);
        chargeState.lowBatteryTriggered = true;
      }
    } else {
      chargeState.lowBatteryTriggered = false;
    }
  }

  // 检查是否所有车辆都空闲
  bool allIdle = true;
  for (auto& pair : m_vehicles) {
    if (pair.second->isControlledByDispatcher()) {
      allIdle = false;
      break;
    }
  }

  if (allIdle && !m_vehicles.empty()) {
    emit vehicleIdle();
  }
}

// ==================== 辅助方法 ====================

void VehicleService::handleStatusUpdate(
    int agvNumber, const kernel::dtos::AgvStatusDTO& status) {
  auto* agv = getVehicle(agvNumber);
  if (!agv) {
    if (m_logger) {
      m_logger->warning(QString("收到未知车辆的状态更新: %1").arg(agvNumber));
    }
    return;
  }

  const bool wasOnline = agv->isOnline();
  const auto now = std::chrono::system_clock::now();

  // 确保空闲计时有初始基准，避免首次上线立即触发空闲充电
  auto& chargeState = m_autoChargeStates[agvNumber];
  if (!chargeState.idleResetTime.has_value()) {
    chargeState.idleResetTime = now;
    agv->updateLastBusyTime();
    chargeState.idleChargeTriggered = false;
    chargeState.lowBatteryTriggered = false;
  }
  // 更新 AgvEntity 的基本信息
  agv->setIpAddress(status.ip.toStdString());
  // 注意：type 在注册时已设置，不需要从 status 中更新
  // 注意：currentMap 需要单独获取，协议中没有此字段

  // 更新位置信息
  kernel::agv::Position pos(status.x, status.y, status.angle);
  agv->setPosition(pos);
  agv->setConfidence(status.confidence);
  agv->setCurrentStation(status.currentStation.toStdString());
  agv->setLastStation(status.lastStation.toStdString());
  agv->setTargetStation(status.targetId.toStdString());  // 使用 targetId

  // 更新电池信息
  agv->setBatteryLevel(status.batteryLevel);
  agv->setChargingStatus(status.charging);
  agv->setCurrentMap(status.current_map.toStdString());
  // 更新状态信息
  agv->setOnlineStatus(true);  // 能收到响应说明在线
  agv->setBlockedStatus(status.blocked);
  // driverEmc / emergency 视为锁定/急停
  agv->setLockedStatus(status.driverEmc || status.emergency);

  // 更新任务状态
  agv->setNavStatus(static_cast<kernel::agv::NavStatus>(status.taskStatus));
  agv->setTaskType(static_cast<kernel::agv::TaskType>(status.taskType));

  // 更新路径信息
  const auto previousFinished = agv->getFinishedPath();
  std::vector<std::string> finishedPath;
  for (const auto& station : status.finishedPath) {
    finishedPath.push_back(station.toStdString());
  }
  agv->setFinishedPath(finishedPath);

  std::vector<std::string> unfinishedPath;
  for (const auto& station : status.unfinishedPath) {
    unfinishedPath.push_back(station.toStdString());
  }
  agv->setUnfinishedPath(unfinishedPath);

  openDoorsForUpcomingSegments(agv, status);
  closeDoorsForFinishedSegments(agv, previousFinished, finishedPath);

  const auto previousAreas = agv->getCurrentAreas();
  std::vector<std::string> areaIds;
  for (const auto& areaId : status.areaIds) {
    areaIds.push_back(areaId.toStdString());
  }

  // [DEBUG] 打印区域状态入口信息
  if (m_logger) {
    QStringList areaList;
    for (const auto& id : status.areaIds) {
      areaList.append(id);
    }
    m_logger->debug(QString("[状态上报入口] AGV%1: areaIds=[%2], currentStation=%3, currentMap=%4")
                        .arg(agvNumber)
                        .arg(areaList.join(", "))
                        .arg(status.currentStation)
                        .arg(status.current_map));
  }

  agv->setCurrentAreas(areaIds);
  updateAgvAreaOccupancy(agv, previousAreas, areaIds, status.current_map);

  // 更新上传时间
  agv->updateLastUploadTime();

  // 车辆刚上线时重置空闲计时与充电触发状态，避免上线瞬间触发空闲充电
  if (!wasOnline && agv->isOnline()) {
    agv->updateLastBusyTime();
    auto& chargeState = m_autoChargeStates[agvNumber];
    chargeState.idleChargeTriggered = false;
    chargeState.lowBatteryTriggered = false;
    chargeState.idleResetTime = now;
  }

  // 发出状态变更信号
  emit vehicleStatusChanged(agvNumber);
}

void VehicleService::openDoorsForUpcomingSegments(
    kernel::agv::AgvEntity* agv, const kernel::dtos::AgvStatusDTO& status) {
  if (!agv) {
    return;
  }
  Q_UNUSED(status);

  const auto& unfinished = agv->getUnfinishedPath();
  if (unfinished.empty()) {
    return;
  }

  auto mapService = m_mapService.lock();
  if (!mapService) {
    return;
  }

  QString mapName = QString::fromStdString(agv->getCurrentMap());
  if (mapName.isEmpty()) {
    return;
  }

  int floor = mapService->getFloorForMapName(mapName);
  if (floor < 0) {
    return;
  }

  auto autoDoorManager = mapService->getAutoDoorManager(floor);
  if (!autoDoorManager) {
    return;
  }

  constexpr int kMaxPoints = 3;
  auto buildPathPoints = [&](const QString& leading) {
    QStringList points;
    if (!leading.isEmpty()) {
      points.append(leading);
    }
    for (const auto& station : unfinished) {
      if (station.empty()) {
        continue;
      }
      points.append(QString::fromStdString(station));
      if (points.size() >= kMaxPoints) {
        break;
      }
    }
    return points;
  };

  auto tryOpenDoors = [&](const QStringList& points, const QString& sourceTag) {
    if (points.size() < 2) {
      return;
    }
    const int availableSegments = static_cast<int>(points.size() - 1);
    const int segmentCount = std::min(2, availableSegments);
    for (int i = 0; i < segmentCount; ++i) {
      const QString& from = points.at(i);
      const QString& to = points.at(i + 1);
      if (from.isEmpty() || to.isEmpty()) {
        continue;
      }

      auto door = autoDoorManager->findDoorBySegment(from, to);
      if (!door) {
        continue;
      }

      autoDoorManager->openDoor(door->getDoorId());
      if (m_logger) {
        m_logger->debug(QString("路径触发自动门(%5): AGV%1, 门ID=%2, 路径=%3->%4")
                            .arg(agv->getNumber())
                            .arg(door->getDoorId())
                            .arg(from)
                            .arg(to)
                            .arg(sourceTag));
      }
    }
  };

  const QString currentStation =
      QString::fromStdString(agv->getCurrentStation());
  const QString lastStation = QString::fromStdString(agv->getLastStation());

  tryOpenDoors(buildPathPoints(currentStation), QStringLiteral("当前站点"));
  if (!lastStation.isEmpty() && lastStation != currentStation) {
    tryOpenDoors(buildPathPoints(lastStation), QStringLiteral("上一站点"));
  }
}

void VehicleService::closeDoorsForFinishedSegments(
    kernel::agv::AgvEntity* agv,
    const std::vector<std::string>& previousFinished,
    const std::vector<std::string>& currentFinished) {
  if (!agv) {
    return;
  }

  if (currentFinished.size() < 2 ||
      currentFinished.size() <= previousFinished.size()) {
    return;
  }

  auto mapService = m_mapService.lock();
  if (!mapService) {
    return;
  }

  QString mapName = QString::fromStdString(agv->getCurrentMap());
  if (mapName.isEmpty()) {
    return;
  }

  int floor = mapService->getFloorForMapName(mapName);
  if (floor < 0) {
    return;
  }

  auto autoDoorManager = mapService->getAutoDoorManager(floor);
  if (!autoDoorManager) {
    return;
  }

  constexpr int kMaxCloseSegments = 2;
  const size_t prevSize = previousFinished.size();
  size_t processed = 0;

  // 只处理新增加的站点所形成的路径段
  for (size_t i = std::max<size_t>(1, prevSize); i < currentFinished.size();
       ++i) {
    if (processed >= kMaxCloseSegments) {
      break;
    }
    const QString from = QString::fromStdString(currentFinished[i - 1]);
    const QString to = QString::fromStdString(currentFinished[i]);
    if (from.isEmpty() || to.isEmpty()) {
      continue;
    }

    auto door = autoDoorManager->findDoorBySegment(from, to);
    if (!door) {
      continue;
    }

    autoDoorManager->closeDoor(door->getDoorId());
    ++processed;

    if (m_logger) {
      m_logger->debug(QString("路径触发自动门关闭: AGV%1, 门ID=%2, 路径=%3->%4")
                          .arg(agv->getNumber())
                          .arg(door->getDoorId())
                          .arg(from)
                          .arg(to));
    }
  }
}

void VehicleService::onUnknownMklmAgvDetected(
    int agvNumber, kernel::dtos::MklmAgvStatusDTO mklmStatus) {
  if (m_logger) {
    m_logger->info(QString("动态注册 MKLM AGV: %1").arg(agvNumber));
  }

  // 创建MKLM命令实现（需要IP地址建立主动连接）
  auto logger = m_logger;  // 可以考虑创建专用logger
  auto commandImpl =
      std::make_shared<infrastructure::vehicle::mklm::MklmCommandImpl>(logger);

  // 使用从状态数据中获取的IP地址注册AGV连接
  if (!commandImpl->registerAgv(agvNumber, mklmStatus.ip)) {
    if (m_logger) {
      m_logger->error(QString("Failed to register MKLM AGV %1 connection to %2")
                          .arg(agvNumber)
                          .arg(mklmStatus.ip));
    }
    return;
  }

  // 注册车辆
  auto registerResult = registerVehicle(agvNumber, commandImpl);
  if (!registerResult.isSuccess()) {
    if (m_logger) {
      m_logger->error(QString("动态注册 MKLM AGV %1 失败: %2")
                          .arg(agvNumber)
                          .arg(registerResult.getErrorMessage()));
    }
    return;
  }

  // 创建MKLM状态提供者
  auto statusLogger = m_logger;  // 可以考虑创建专用logger
  auto statusProvider =
      std::make_shared<infrastructure::vehicle::mklm::MklmStatusProvider>(
          agvNumber, statusLogger);

  // 注册状态提供者
  auto statusResult = registerVehicleStatusProvider(agvNumber, statusProvider);
  if (!statusResult.isSuccess()) {
    if (m_logger) {
      m_logger->error(QString("动态注册 MKLM AGV %1 状态提供者失败: %2")
                          .arg(agvNumber)
                          .arg(statusResult.getErrorMessage()));
    }
    unregisterVehicle(agvNumber);
    return;
  }

  // 使用接收到的初始状态更新车辆
  // Convert MklmAgvStatusDTO to AgvStatusDTO
  kernel::dtos::AgvStatusDTO status;
  status.agvNumber = mklmStatus.agvNumber;
  status.ip = mklmStatus.ip;
  status.mode = mklmStatus.mode;
  status.x = mklmStatus.x;
  status.y = mklmStatus.y;
  status.angle = mklmStatus.angle;
  status.confidence = mklmStatus.confidence;
  status.currentStation = mklmStatus.currentStation;
  status.lastStation = mklmStatus.lastStation;
  status.current_map = mklmStatus.currentMap;
  status.blocked = mklmStatus.blocked;
  status.areaIds = mklmStatus.areaIds;

  status.taskStatus = mklmStatus.taskStatus;
  status.taskType = mklmStatus.taskType;
  /*  status.finishedPath =
   * QJsonArray::fromStringList(mklmStatus.finishedPath);
 status.unfinishedPath
   * = QJsonArray::fromStringList(mklmStatus.unfinishedPath);*/

  handleStatusUpdate(agvNumber, status);

  if (m_logger) {
    m_logger->info(QString("MKLM AGV %1 动态注册成功").arg(agvNumber));
  }
}

bool VehicleService::vehicleExists(int agvNumber) const {
  return m_vehicles.find(agvNumber) != m_vehicles.end();
}

bool VehicleService::isSameRoute(
    const std::vector<std::string>& unfinishedPath,
    const std::vector<std::string>& newStations) const {
  // 参考 agv.cpp:145-171
  if (unfinishedPath.empty() || newStations.empty()) {
    return false;
  }

  // 从后往前比对：首先检查最后一个站点是否相同
  if (unfinishedPath.back() != newStations.back()) {
    return false;
  }

  // 检查unfinished_path是否小于等于新路径
  if (unfinishedPath.size() > newStations.size()) {
    return false;
  }

  // 从后往前逐个比对
  int unfinishedIndex = unfinishedPath.size() - 1;
  for (int i = newStations.size() - 1; i >= 0 && unfinishedIndex >= 0; --i) {
    if (unfinishedPath[unfinishedIndex] != newStations[i]) {
      return false;
    }
    unfinishedIndex--;
  }

  return true;
}

int VehicleService::calculateRouteStartIndex(
    const std::vector<std::string>& finishedPath,
    const std::vector<std::string>& newStations) const {
  // 参考 agv.cpp:173-188
  if (finishedPath.empty() || newStations.empty()) {
    return 0;
  }

  int startIndex = 0;

  // 从后往前遍历finished_path
  for (int i = finishedPath.size() - 1; i >= 0; --i) {
    // 查找新路径的第一个站点在finished_path中的位置
    if (newStations[0] == finishedPath[i]) {
      // 方向性检查：如果finished_path中有前一个站点，检查是否与新路径第二个站点匹配
      if (i > 1 && newStations.size() > 1) {
        if (newStations[1] == finishedPath[i - 1]) {
          break;  // 方向相反，不跳过
        }
      }
      startIndex++;
      break;
    }
  }

  return startIndex;
}

kernel::dtos::AgvInfoDTO VehicleService::entityToInfoDTO(
    const kernel::agv::AgvEntity* entity) const {
  if (!entity) {
    return kernel::dtos::AgvInfoDTO();
  }

  kernel::dtos::AgvInfoDTO dto;

  // 基本信息
  dto.agvNumber = entity->getNumber();
  dto.name = QString("AGV%1").arg(entity->getNumber(), 3, 10, QChar('0'));
  dto.ip = QString::fromStdString(entity->getIpAddress());
  dto.type = static_cast<int>(entity->getType());

  // 位置信息
  dto.x = entity->getPosition().x;
  dto.y = entity->getPosition().y;
  dto.angle = entity->getPosition().angle;
  dto.currentStation = QString::fromStdString(entity->getCurrentStation());
  dto.lastStation = QString::fromStdString(entity->getLastStation());
  dto.targetStation = QString::fromStdString(entity->getTargetStation());
  dto.currentMap = QString::fromStdString(entity->getCurrentMap());

  for (const auto& area : entity->getCurrentAreas()) {
    dto.areaIds.append(QString::fromStdString(area));
  }

  // 状态信息
  dto.power = entity->getBatteryLevel();
  dto.confidence = entity->getConfidence();
  dto.charging = entity->isCharging();
  dto.locked = entity->isLocked();
  dto.blocked = entity->isBlocked();
  dto.online = entity->isOnline();
  dto.mode = 1;  // 默认自动模式
  dto.liftStage = 0;

  // 任务信息
  dto.taskStatus = static_cast<int>(entity->getNavStatus());
  dto.taskType = static_cast<int>(entity->getTaskType());
  dto.taskListCount = 0;  // TODO: 如果需要任务列表数量，需要从entity获取
  dto.isControlledByDispatcher = entity->isControlledByDispatcher();

  for (const auto& station : entity->getFinishedPath()) {
    dto.finishedPath.append(QString::fromStdString(station));
  }
  for (const auto& station : entity->getUnfinishedPath()) {
    dto.unfinishedPath.append(QString::fromStdString(station));
  }

  // 错误信息
  // TODO: 如果entity有错误信息，需要添加

  // 时间戳
  dto.uploadTime = QDateTime::fromSecsSinceEpoch(
      std::chrono::system_clock::to_time_t(entity->getLastUploadTime()));
  dto.lastBusyTime = QDateTime::fromSecsSinceEpoch(
      std::chrono::system_clock::to_time_t(entity->getLastBusyTime()));

  return dto;
}

void VehicleService::updateAgvAreaOccupancy(
    kernel::agv::AgvEntity* agv,
    const std::vector<std::string>& previousAreas,
    const std::vector<std::string>& currentAreas,
    const QString& currentMapName)
{
  if (!agv) {
    return;
  }

  auto mapService = m_mapService.lock();
  if (!mapService) {
    return;
  }

  QString resolvedMapName = currentMapName;
  if (resolvedMapName.isEmpty()) {
    resolvedMapName = QString::fromStdString(agv->getCurrentMap());
  }

  const int currentFloor = resolvedMapName.isEmpty()
      ? -1
      : mapService->getFloorForMapName(resolvedMapName);
  auto currentManager = currentFloor >= 0
      ? mapService->getAreaManager(currentFloor)
      : std::shared_ptr<application::area::AreaManager>();

  const auto buildSet = [](const std::vector<std::string>& areas) {
    QSet<QString> set;
    for (const auto& id : areas) {
      if (!id.empty()) {
        set.insert(QString::fromStdString(id));
      }
    }
    return set;
  };

  const bool hadSnapshot = m_agvAreaSnapshots.contains(agv->getNumber());
  const auto snapshot = hadSnapshot
      ? m_agvAreaSnapshots.value(agv->getNumber())
      : AgvAreaSnapshot();
  QSet<QString> previousSet = snapshot.areaIds;
  if (hadSnapshot && previousSet.isEmpty()) {
    previousSet = buildSet(previousAreas);
  }
  const QSet<QString> currentSet = buildSet(currentAreas);

  // [DEBUG] 打印区域更新逻辑关键信息
  if (m_logger) {
    QStringList prevList = previousSet.values();
    QStringList currList = currentSet.values();
    m_logger->debug(QString("[区域更新逻辑] AGV%1: previousSet=[%2], currentSet=[%3], currentFloor=%4, hasManager=%5")
                        .arg(agv->getNumber())
                        .arg(prevList.join(", "))
                        .arg(currList.join(", "))
                        .arg(currentFloor)
                        .arg(currentManager ? "YES" : "NO"));
  }

  const int previousFloor = (hadSnapshot && snapshot.floor >= 0)
      ? snapshot.floor
      : currentFloor;
  auto previousManager = previousFloor >= 0
      ? mapService->getAreaManager(previousFloor)
      : std::shared_ptr<application::area::AreaManager>();

  for (const QString& areaId : previousSet) {
    if (currentSet.contains(areaId)) {
      continue;
    }
    if (previousManager && previousManager->hasArea(areaId)) {
      previousManager->exitArea(areaId, agv->getNumber());
    }
  }

  if (currentManager) {
    for (const QString& areaId : currentSet) {
      if (previousSet.contains(areaId)) {
        continue;
      }
      if (currentManager->hasArea(areaId)) {
        // [DEBUG] 打印进入区域的调用
        if (m_logger) {
          m_logger->debug(QString("[区域更新逻辑] AGV%1 调用 enterArea: areaId=%2")
                              .arg(agv->getNumber())
                              .arg(areaId));
        }
        currentManager->enterArea(areaId, agv->getNumber());
      } else {
        // [DEBUG] 区域不存在
        if (m_logger) {
          m_logger->debug(QString("[区域更新逻辑] AGV%1 区域不存在: areaId=%2")
                              .arg(agv->getNumber())
                              .arg(areaId));
        }
      }
    }
  }

  if (!currentSet.isEmpty() && currentFloor >= 0 && currentManager) {
    m_agvAreaSnapshots.insert(agv->getNumber(), AgvAreaSnapshot{currentFloor, currentSet});
  } else {
    m_agvAreaSnapshots.remove(agv->getNumber());
  }
}

void VehicleService::clearAgvAreaOccupancy(int agvNumber)
{
  if (!m_agvAreaSnapshots.contains(agvNumber)) {
    return;
  }

  const auto snapshot = m_agvAreaSnapshots.take(agvNumber);
  if (snapshot.areaIds.isEmpty()) {
    return;
  }

  auto mapService = m_mapService.lock();
  if (!mapService) {
    return;
  }

  if (auto areaManager = mapService->getAreaManager(snapshot.floor)) {
    for (const auto& areaId : snapshot.areaIds) {
      if (!areaId.isEmpty()) {
        areaManager->exitArea(areaId, agvNumber);
      }
    }
  }
}

void VehicleService::onHealthCheckTimeout() {
  if (m_vehicles.empty()) {
    return;
  }

  updateVehicleOnlineStatus();
  manageAutoCharging();
}

}  // namespace vehicle
}  // namespace application
